class_name SteeringBehaviorPath
extends Path2D

class Segment:
	var begin:Vector2
	var end:Vector2
	var length:float
	# 累计长度，用于表示整个Path的起始点到当前end的距离
	var cumulative_length:float
	func _init(begin:Vector2,end:Vector2) -> void:
		self.begin = begin
		self.end = end
		self.length = self.begin.distance_to(self.end)
#region 属性
@export var path_width:float = 0
var is_open:bool:
	get:
		if curve.point_count < 2:
			return false
		return curve.get_point_position(0) != curve.get_point_position(curve.point_count - 1)
var length:float = 0
var segments:Array[Segment] = []
var nearest_point_on_segment: Vector2
var nearest_point_on_path: Vector2
var target_position:Vector2
#endregion
#region 字段
#endregion
#region 信号
#endregion

#region 基类方法
#endregion
#region 公共方法
## 计算点到path的最小距离
func calculate_min_vertical_distance(point_position:Vector2) -> Array:
	var segment:Segment
	var min_distance:float = INF
	for i in range(segments.size()):
		var s = segments[i]
		var distance = _calculate_point_segment_distance(s.begin,s.end,point_position)
		if distance < min_distance:
			min_distance = distance
			segment = s
	return [segment,min_distance]
## 计算垂点距离path的start的距离
func calculate_distance(point_position:Vector2) -> float:
	var segment:Segment = calculate_min_vertical_distance(point_position)[0]
	if not segment:
		return 0
	nearest_point_on_path = nearest_point_on_segment
	var length_on_path = segment.cumulative_length - nearest_point_on_path.distance_to(segment.end)
	return length_on_path
func calculate_target_position(agent: SteeringBehaviorAgent,behavior:ContextBehavior_FollowPath,target_distance: float) -> Vector2:
	if is_open:
		target_distance = clamp(target_distance, 0.0, length)
	else:
		if target_distance < 0:
			target_distance = length + fmod(target_distance, length)
		elif target_distance > length:
			target_distance = fmod(target_distance, length)
	var segment:Segment
	for i in range(segments.size()):
		var s = segments[i]
		if s.cumulative_length >= target_distance:
			segment = s
			break
	if not segment:
		segment = segments[-1]
	var distance = segment.cumulative_length - target_distance
	target_position = segment.end + (segment.begin - segment.end) * clamp(distance / segment.length,0,1)
	var dis = calculate_min_vertical_distance(agent.get_pos())[1]
	if dis <= path_width:
		var start_to_end = segment.end - segment.begin
		var point_to_target = target_position - agent.get_pos()
		if segment.begin.distance_to(agent.get_pos()) > segment.length:
			point_to_target = Vector2.ZERO
		var projection_length = pow(point_to_target.dot(agent.get_pos().normalized() + start_to_end.normalized()),0.5)
		target_position = agent.get_pos() + start_to_end.normalized() * abs(projection_length)
	return target_position
#endregion
#region 私有方法
## 计算点到线段上的点的最短距离
func _calculate_point_segment_distance(start:Vector2,end:Vector2,position:Vector2) -> float:
	nearest_point_on_segment = start
	var start_to_end = end - start
	var start_to_end_length_squared = start_to_end.length_squared()
	if start_to_end_length_squared != 0:
		var t = (position - start).dot(start_to_end)/start_to_end_length_squared
		nearest_point_on_segment += start_to_end * clamp(t,0.0,1.0)
	return nearest_point_on_segment.distance_to(position)
## 计算点在线段上的垂点
func _calculate_point_segment_vertical_point(start:Vector2,end:Vector2,position:Vector2) -> Vector2:
	nearest_point_on_segment = start
	var start_to_end = end - start
	var start_to_end_length_squared = start_to_end.length_squared()
	if start_to_end_length_squared != 0:
		var t = (position - start).dot(start_to_end)/start_to_end_length_squared
		nearest_point_on_segment += start_to_end * clamp(t,0.0,1.0)
	return nearest_point_on_segment
#endregion
#region 生命周期函数
func _init() -> void:
	length = 0
	segments.clear()
	if curve.point_count < 2:
		return
	var current:Vector2 = curve.get_point_position(0)
	var previous:Vector2
	for i in range(1,curve.point_count):
		previous = current
		current = curve.get_point_position(i)
		var segment = Segment.new(to_global(previous),to_global(current))
		length += segment.length
		segment.cumulative_length = length
		segments.append(segment)
func _process(delta: float) -> void:
	queue_redraw()
func _draw() -> void:
	draw_polyline(curve.get_baked_points(),Color.GRAY,path_width)
	draw_circle(target_position,6,Color.CYAN,true)
#endregion
